/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: IMUData.cpp,v 1.8 2009-05-13 07:18:16 amaula Exp $
 *
 * \file
 * \brief Implementatino for IMU data encoder/decoder
 * \author Matthieu Myrsky
 */
#include "IMUData.hpp"
#include "binbag.h"
#include <stdio.h>
#include "owndebug.h"
//*****************************************************************************
//*****************************************************************************
using namespace MaCI::IMU;
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************


CIMUData::CIMUData()
 : iTimestampPtr(NULL),
   iAccelerationPtr(NULL),
   iAngularVelocityPtr(NULL),
   iAnglePtr(NULL),
   iQuaternionPtr(NULL),
   iOrientationMatrixPtr(NULL),
   iEventTypesPtr(NULL),
   iCommandPtr(NULL)
{
}
//*****************************************************************************

CIMUData::~CIMUData()
{
}
//*****************************************************************************

CIMUData::CIMUData(const CIMUData &aData)
 : iTimestampPtr(NULL),
   iAccelerationPtr(NULL),
   iAngularVelocityPtr(NULL),
   iAnglePtr(NULL),
   iQuaternionPtr(NULL),
   iOrientationMatrixPtr(NULL),
   iEventTypesPtr(NULL),
   iCommandPtr(NULL)
{
  // Call assignment operator.
  *this = aData;
}
//*****************************************************************************

CIMUData &CIMUData::operator=(const CIMUData &aData)
{
  // Reset
  Reset();

  // Create an internal BinBag instance
  if (CreateInternalBinBag(aData.iBinBagContainer) == false) {
    dPrint(4,"WARNING: Failed to create internal binbag for object %p from object %p",
           this, &aData);
  }
  
  // Do the decode on the buffer; it sets pointers and
  if (DecodeFrom() == false) {
    dPrint(4,"Warning: Failed to Decode CPositionData even though assignment operator used?");
  }
  
  return *this; 


}
//*****************************************************************************

bool CIMUData::DecodeFrom(gim::binbag::CBinBag *aBinBag)
{
 bool result = true;
  const gim::binbag::TBinBlob *b = NULL;
  int previous_type = -1;
  
  // Store pointer and test it for NullNess
  if (aBinBag != NULL)  
    iBinBagContainer = aBinBag;
  
  // Parse
  if (iBinBagContainer && iBinBagContainer->Rewind()) {
    while( (b = iBinBagContainer->GetNextBinary()) && result ) {
      switch(b->type) {
      case KTypeTimestamp:
	if (iTimestampPtr == NULL) {
	  iTimestampPtr = reinterpret_cast<const Common::TTimestamp *>(b->data);
	} else { 
          dPrint(10,"Decoding Error: iTimestampPtr was not NULL");
          result = false; 
        }
	break;

      case KTypeAcceleration3d:
	if(iAccelerationPtr == NULL) {
          iAccelerationPtr = reinterpret_cast<const TAcceleration3d *>(b->data);
        } else { 
          dPrint(10,"Decoding Error: iAccelerationPtr was not NULL");
          result = false; 
        }
        break;

      case KTypeAngularVelocity3d:
	if(iAngularVelocityPtr == NULL) {
          iAngularVelocityPtr = reinterpret_cast<const TAngularVelocity3d *>(b->data);
        } else { 
          dPrint(10,"Decoding Error: iAngularVelocityPtr was not NULL");
          result = false; 
        }
        break;

      case KTypeAngle3d:
	if(iAnglePtr == NULL) {
          iAnglePtr = reinterpret_cast<const TAngle3d *>(b->data);
        } else { 
          dPrint(10,"Decoding Error: iAnglePtr was not NULL");
          result = false; 
        }
        break;

      case KTypeQuaternion:
	if(iQuaternionPtr == NULL) {
          iQuaternionPtr = reinterpret_cast<const TQuaternion *>(b->data);
        } else { 
          dPrint(10,"Decoding Error: iQuaternionPtr was not NULL");
          result = false; 
        }
        break;

      case KTypeOrientationMatrix:
	if(iOrientationMatrixPtr == NULL) {
          iOrientationMatrixPtr = reinterpret_cast<const TOrientationMatrix *>(b->data);
        } else { 
          dPrint(10,"Decoding Error: iOrientationMatrixPtr was not NULL");
          result = false; 
        }
        break;

      case KTypeCommand:
        if (iCommandPtr == NULL) {
          iCommandPtr = reinterpret_cast<const TCommand *>(b->data);
        } else { 
          dPrint(10,"Decoding Error: iCommandPtr was not NULL");
          result = false; 
        }
        break;
      case KTypeEventTypes:
        if (iEventTypesPtr == NULL) {
          iEventTypesPtr = reinterpret_cast<const TEventTypes *>(b->data);
        } else { 
          dPrint(10,"Decoding Error: iEventTypesPtr was not NULL");
          result = false; 
        }

        break;
      default:
	// Unknown Type in BinBag. Do nothing.
	break;
      }
      
      // Store previous type
      previous_type = b->type;
    }
  } else {
    result = false;
  }
  
  // Store the result
  SetReadOnlyContainer(result);
  
  return result;
}
//*****************************************************************************

void CIMUData::Reset()
{
  CMaCIData::Reset();

  iTimestampPtr = NULL;
  iAccelerationPtr = NULL;
  iAngularVelocityPtr = NULL;
  iAnglePtr = NULL;
  iQuaternionPtr = NULL;
  iOrientationMatrixPtr= NULL;
  iCommandPtr = NULL;
  iEventTypesPtr = NULL;
}
//*****************************************************************************

bool CIMUData::SetTimestamp(const Common::TTimestamp &aStamp){
 bool result = false;
  if (!IsReadOnlyContainer() && iTimestampPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeTimestamp, 
                                  (const char *)&aStamp, 
                                  sizeof(Common::TTimestamp));
    iTimestampPtr = reinterpret_cast<const Common::TTimestamp *>(b->data);

    // Mark last element
    SetLastElementType( KTypeTimestamp);

    result = true;
  }
  return result;
}
//*****************************************************************************

bool CIMUData::SetCommand(const TCommand &aCommand){
  bool result = false;
  if (!IsReadOnlyContainer() && iCommandPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeCommand, 
                                  (const char *)&aCommand, 
                                  sizeof(TCommand));
    iCommandPtr = reinterpret_cast<const TCommand *>(b->data);

    // Mark last element
    SetLastElementType( KTypeCommand);

    result = true;
  }
  return result;
}
//*****************************************************************************

bool CIMUData::SetAcceleration3d(const TAcceleration3d &aAcceleration){
  bool result = false;
  if (!IsReadOnlyContainer() && iAccelerationPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeAcceleration3d, 
                                  (const char *)&aAcceleration, 
                                  sizeof(TAcceleration3d));
    iAccelerationPtr = reinterpret_cast<const TAcceleration3d *>(b->data);

    // Mark last element
    SetLastElementType( KTypeAcceleration3d);

    result = true;
  }
  return result;
}
//*****************************************************************************

bool CIMUData::SetAngularVelocity3d(const TAngularVelocity3d &aAngularVelocity){
  bool result = false;
  if (!IsReadOnlyContainer() && iAngularVelocityPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeAngularVelocity3d, 
                                  (const char *)&aAngularVelocity, 
                                  sizeof(TAngularVelocity3d));
    iAngularVelocityPtr = reinterpret_cast<const TAngularVelocity3d *>(b->data);

    // Mark last element
    SetLastElementType( KTypeAngularVelocity3d);

    result = true;
  }
  return result;
}
//*****************************************************************************

bool CIMUData::SetAngle3d(const TAngle3d &aAngle){
 bool result = false;
  if (!IsReadOnlyContainer() && iAnglePtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeAngle3d, 
                                  (const char *)&aAngle, 
                                  sizeof(TAngle3d));
    iAnglePtr = reinterpret_cast<const TAngle3d *>(b->data);

    // Mark last element
    SetLastElementType( KTypeAngle3d);

    result = true;
  }
  return result;
}
//*****************************************************************************

bool CIMUData::SetQuaternion(const TQuaternion &aQuaternion){
  bool result = false;
  if (!IsReadOnlyContainer() && iQuaternionPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeQuaternion, 
                                  (const char *)&aQuaternion, 
                                  sizeof(TQuaternion));
    iQuaternionPtr = reinterpret_cast<const TQuaternion *>(b->data);

    // Mark last element
    SetLastElementType( KTypeQuaternion);

    result = true;
  }
  return result;
}
//*****************************************************************************
   
bool CIMUData::SetOrientationMatrix(const TOrientationMatrix &aMatrix){
  bool result = false;
  if (!IsReadOnlyContainer() && iOrientationMatrixPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeOrientationMatrix, 
                                  (const char *)&aMatrix, 
                                  sizeof(TOrientationMatrix));
    iOrientationMatrixPtr = reinterpret_cast<const TOrientationMatrix *>(b->data);

    // Mark last element
    SetLastElementType( KTypeOrientationMatrix);

    result = true;
  }
  return result;


}
//*****************************************************************************

bool CIMUData::SetEventTypes(const TEventTypes &aEventTypes){
  bool result = false;
  if (!IsReadOnlyContainer() && iEventTypesPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeEventTypes, 
                                  (const char *)&aEventTypes, 
                                  sizeof(TEventTypes));
    iEventTypesPtr = reinterpret_cast<const TEventTypes *>(b->data);

    // Mark last element
    SetLastElementType( KTypeEventTypes);

    result = true;
  }
  return result;


}
//*****************************************************************************

void CIMUData::Print(const int level) const{

  const gim::binbag::TBinBlob *b = NULL;
  // Parse
  if (iBinBagContainer && iBinBagContainer->Rewind()) {
    while( (b = iBinBagContainer->GetNextBinary())) {
      switch(b->type) {
      case KTypeTimestamp: {
	const Common::TTimestamp *ts = 
          reinterpret_cast<const Common::TTimestamp *>(b->data);
	dPrint(level,"Common::TTimestamp : %d, %d", 
	       ts->timestamp_s,
	       ts->timestamp_us);
	break;
      }

      case KTypeAcceleration3d:{
	const TAcceleration3d *ta = 
          reinterpret_cast<const TAcceleration3d *>(b->data);
	dPrint(level,"TAcceleration3d = (%f,%f,%f)",
	       ta->accelerationX,
	       ta->accelerationY,
	       ta->accelerationZ);
	
	break;
      }
      case KTypeAngularVelocity3d:{
	const TAngularVelocity3d *tr = 
          reinterpret_cast<const TAngularVelocity3d *>(b->data);
	dPrint(level,"TAngularVelocity3d =  (%f,%f,%f)",
	       tr->wX,
	       tr->wY,
	       tr->wZ);
	
	break;

      }
      case KTypeAngle3d:{
	const TAngle3d *tan = 
          reinterpret_cast<const TAngle3d *>(b->data);
	dPrint(level,"TAngle3d = (%f,%f,%f)",
	       tan->roll,
	       tan->pitch,
	       tan->yaw);
	break;
      }
      case KTypeQuaternion:{
	const TQuaternion *tq = 
          reinterpret_cast<const TQuaternion *>(b->data);
	dPrint(level,"TQuaternion = (%f,%f,%f,%f)",
	       tq->scalar,
	       tq->x,
	       tq->y,
	       tq->z);
	break;
      }
      case KTypeOrientationMatrix:{
	const TOrientationMatrix *tom = 
          reinterpret_cast<const TOrientationMatrix *>(b->data);
	dPrint(level,"TOrientationMatrix =\n|%f,%f,%f|\n|%f,%f,%f|\n|%f,%f,%f|",
	       tom->e11,
	       tom->e12,
	       tom->e13,
	       tom->e21,
	       tom->e22,
	       tom->e23,
	       tom->e31,
	       tom->e32,
	       tom->e33);
	break;
      }
      case KTypeCommand: {
        const TCommand *cp = reinterpret_cast<const TCommand *>(b->data);
        dPrint(level,"TCommand: %d",
               cp->cmd);
        break;
      }
      case KTypeEventTypes:{
        const TEventTypes  *et = reinterpret_cast<const TEventTypes *>(b->data);
        dPrint(level,"EventTypes: 0x%x",et->continuousTypes);
        break;
      }
	
      default:{
	dPrint(level,"Unknown Type %d",b->type);
	break;
      }
    }
  }
}
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
#ifdef COMPILE_IMUDATA_STANDALONE_MAIN
using namespace MaCI::IMU;

int main(void)
{
  gim::binbag::CBinBag bb;
  CIMUData A;
  CIMUData B;
  debugInit();
  printf("Carry out Encoding/Decoding test.\n");

  printf("Encoding...\n");
  // Do Encoding 
  A.EncodeTo(&bb);
  printf("Encode: Timestamp(1,2)...");
  A.SetTimestamp(MaCI::Common::TTimestamp(1, 2));
  printf("OK.\n");

  printf("Encode: Acceleration3d(1.1,2.2,3.3)...");
  A.SetAcceleration3d(TAcceleration3d(1.1,2.2,3.3));
  printf("OK.\n");

  printf("Encode: AngularVelocity3d(2.2,3.3,4.4)...");
  A.SetAngularVelocity3d(TAngularVelocity3d(2.2,3.3,4.4));
  printf("OK.\n");

  printf("Encode: TAngle3D(3.3,4.4,5.5)...");
  A.SetAngle3d(TAngle3d(3.3,4.4,5.5));
  printf("OK.\n");

  printf("Encode: TQuaternion(1.2,2.3,3.4,4.5)...");
  A.SetQuaternion(TQuaternion(1.2,2.3,3.4,4.5));
  printf("OK.\n");

  printf("Encode: TOrientationMatrix:(0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9)...");
  A.SetOrientationMatrix(TOrientationMatrix(0.1,0.2,0.3,0.4,0.5,0.6,0.7,0.8,0.9));
  printf("OK.\n");

  printf("-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*\n");
  printf("Testing the print function\n");
  A.Print(1);
  printf("-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*\n");
  printf("Decoding....\n");
  // Do Decoding
  B.DecodeFrom(&bb);
  printf("Decode: Timestamp = (%d, %d)\n", 
	 B.GetTimestamp()->timestamp_s, 
	 B.GetTimestamp()->timestamp_us);
  
  printf("Decode: TAcceleration3d = (%f,%f,%f)\n",
	 B.GetAcceleration3d()->accelerationX,
	 B.GetAcceleration3d()->accelerationY,
	 B.GetAcceleration3d()->accelerationZ);

  printf("Decode: TAngularVelocity3d =  (%f,%f,%f)\n",
	 B.GetAngularVelocity3d()->wX,
	 B.GetAngularVelocity3d()->wY,
	 B.GetAngularVelocity3d()->wZ);

  printf("Decode: TAngle3d = (%f,%f,%f)\n",
	 B.GetAngle3d()->roll,
	 B.GetAngle3d()->pitch,
	 B.GetAngle3d()->yaw);

  printf("Decode: TQuaternion = (%f,%f,%f,%f)\n",
	 B.GetQuaternion()->scalar,
	 B.GetQuaternion()->x,
	 B.GetQuaternion()->y,
	 B.GetQuaternion()->z);

  printf("Decode: TOrientationMatrix = (%f,%f,%f,%f,%f,%f,%f,%f,%f)\n",
	 B.GetOrientationMatrix()->e11,
	 B.GetOrientationMatrix()->e12,
	 B.GetOrientationMatrix()->e13,
	 B.GetOrientationMatrix()->e21,
	 B.GetOrientationMatrix()->e22,
	 B.GetOrientationMatrix()->e23,
	 B.GetOrientationMatrix()->e31,
	 B.GetOrientationMatrix()->e32,
	 B.GetOrientationMatrix()->e33);
  printf("-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*\n");
  printf("\nCopying with copy-constructor and Re-Decoding...\n");
  CIMUData C(B);
  printf("Decode: Timestamp = (%d, %d)\n", 
	 C.GetTimestamp()->timestamp_s, 
	 C.GetTimestamp()->timestamp_us);
  
  printf("Decode: TAcceleration3d = (%f,%f,%f)\n",
	 C.GetAcceleration3d()->accelerationX,
	 C.GetAcceleration3d()->accelerationY,
	 C.GetAcceleration3d()->accelerationZ);

  printf("Decode: TAngularVelocity3d =  (%f,%f,%f)\n",
	 C.GetAngularVelocity3d()->wX,
	 C.GetAngularVelocity3d()->wY,
	 C.GetAngularVelocity3d()->wZ);

  printf("Decode: TAngle3d = (%f,%f,%f)\n",
	 C.GetAngle3d()->roll,
	 C.GetAngle3d()->pitch,
	 C.GetAngle3d()->yaw);

  printf("Decode: TQuaternion = (%f,%f,%f,%f)\n",
	 C.GetQuaternion()->scalar,
	 C.GetQuaternion()->x,
	 C.GetQuaternion()->y,
	 C.GetQuaternion()->z);

  printf("Decode: TOrientationMatrix = (%f,%f,%f,%f,%f,%f,%f,%f,%f)\n",
	 C.GetOrientationMatrix()->e11,
	 C.GetOrientationMatrix()->e12,
	 C.GetOrientationMatrix()->e13,
	 C.GetOrientationMatrix()->e21,
	 C.GetOrientationMatrix()->e22,
	 C.GetOrientationMatrix()->e23,
	 C.GetOrientationMatrix()->e31,
	 C.GetOrientationMatrix()->e32,
	 C.GetOrientationMatrix()->e33);
  printf("-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*\n");
  CIMUData D = C;
  printf("Trying the = operator\n");
  printf("Decode: Timestamp = (%d, %d)\n", 
	 D.GetTimestamp()->timestamp_s, 
	 D.GetTimestamp()->timestamp_us);
  
  printf("Decode: TAcceleration3d = (%f,%f,%f)\n",
	 D.GetAcceleration3d()->accelerationX,
	 D.GetAcceleration3d()->accelerationY,
	 D.GetAcceleration3d()->accelerationZ);

  printf("Decode: TAngularVelocity3d =  (%f,%f,%f)\n",
	 D.GetAngularVelocity3d()->wX,
	 D.GetAngularVelocity3d()->wY,
	 D.GetAngularVelocity3d()->wZ);

  printf("Decode: TAngle3d = (%f,%f,%f)\n",
	 D.GetAngle3d()->roll,
	 D.GetAngle3d()->pitch,
	 D.GetAngle3d()->yaw);

  printf("Decode: TQuaternion = (%f,%f,%f,%f)\n",
	 D.GetQuaternion()->scalar,
	 D.GetQuaternion()->x,
	 D.GetQuaternion()->y,
	 D.GetQuaternion()->z);

  printf("Decode: TOrientationMatrix = (%f,%f,%f,%f,%f,%f,%f,%f,%f)\n",
	 D.GetOrientationMatrix()->e11,
	 D.GetOrientationMatrix()->e12,
	 D.GetOrientationMatrix()->e13,
	 D.GetOrientationMatrix()->e21,
	 D.GetOrientationMatrix()->e22,
	 D.GetOrientationMatrix()->e23,
	 D.GetOrientationMatrix()->e31,
	 D.GetOrientationMatrix()->e32,
	 D.GetOrientationMatrix()->e33);
  printf("\nTest complete.\n");
  return 0;
  

}
#endif
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
